#coding=utf-8
import numpy as np
from PySide2.QtCore import Qt

import app
import basic_math.geolib
from basic_math import *
from controller import geometric,kinematic
import pyqtgraph as pg
import pyqtgraph.opengl as gl
from PySide2 import QtGui

class RobotModelWidget(gl.GLViewWidget):
    '''
        用于显示足底压力的类(双足)
    '''
    def __init__(self,rootObject,
                 showStatic=True,showChain=True,showTerminal=True,showPhysical=True,
                 envColor=(80,80,80)):
        super().__init__()

        #状态Flag
        self.showStatic=showStatic
        self.showChain=showChain
        self.showTerminal=showTerminal
        self.showPhysical=showPhysical
        self.envColor=envColor
        self.SetView()
        self.rootObject=rootObject

    def SetView(self,view='x'):
        if view=='x':
            self.setCameraPosition(distance=2, elevation=0, azimuth=0)
            self.opts['view'] = 'yz'
        if view=='y':
            self.opts['view'] = 'xz'
            self.setCameraPosition(distance=2, elevation=0, azimuth=90)
        if view=='z':
            self.opts['view'] = 'xy'
            self.setCameraPosition(distance=2, elevation=90, azimuth=180)
    def RenderRobot(self):
        '''
        渲染场景中的机器人
        :param terminals:
        :param chains:
        :return:
        '''
        self.clear()    #删除所有的项目
        self.setBackgroundColor(self.envColor)
        if self.showStatic: self.RenderStatic()
        if self.showTerminal:
            self.RenderTerminal([self.rootObject.lFoot,self.rootObject.rFoot],self.rootObject.base)
            if self.showPhysical:
                self.RenderTerminal(self.rootObject.physical.GetFoots(),self.rootObject.physical.GetBase(),color=(0.7,0.7,1,0.1))
        if self.showChain:
            self.RenderChain([self.rootObject.lChain,self.rootObject.rChain])


    def RenderStatic(self):
        '''
            添加固定元素
        '''
        self.addItem(gl.GLTextItem(pos=(0.2, 0, 0), text="X",font=QtGui.QFont("Arial", 8)))
        self.addItem(gl.GLTextItem(pos=(0.0, 0.2, 0), text="Y",font=QtGui.QFont("Arial", 8)))
        self.addItem(gl.GLTextItem(pos=(0.0, 0.0, 0.2), text="Z",font=QtGui.QFont("Arial", 8)))
        self.addItem(gl.GLAxisItem())
        plate = gl.GLGridItem()
        plate.setSize(0.5, 0.5, 0.5)
        plate.setSpacing(0.01, 0.01, 0.01)
        plate.translate(0,0,0)
        self.addItem(plate)


    def RenderTerminal(self,terminals,base,color=(0.8,0.8,0.8,0.1),colorActive=(0.6, 1, 0.6, 0.1)):
        '''
            渲染机器人脚底末端以及base_link
        '''
        for terminal in terminals:
            '''
                添加脚部Terminal
            '''
            if terminal.isSupport:
                terminalBox = self.GenBoxMesh(terminal.size, terminal.VectorPose(), terminal.roll, terminal.pitch,
                                              terminal.yaw, colorActive)
            else:
                terminalBox=self.GenBoxMesh(terminal.size,terminal.VectorPose(),terminal.roll,terminal.pitch,terminal.yaw,color)
            self.addItem(terminalBox)

            '''
                添加base部分的Terminal
            '''
        baseBox = self.GenBoxMesh(base.size, base.VectorPose(), base.roll, base.pitch, base.yaw, color)
        self.addItem(baseBox)
        
    def RenderChain(self,chains,color=(0.7,0.7,0.7,0.3),textColor=(255, 255, 0, 255)):
        '''
            渲染机器人各个关节的链条
        '''
        for limb in chains:
            for i in range(len(limb.chain)):
                link=limb.chain[i]
                bone=self.GenBoxMesh(link.size,link.CenterPose(),link.start.roll,link.start.pitch,link.start.yaw,color)
                self.addItem(bone)



    def GenBoxMesh(self,size,pos,roll,pitch,yaw,color=(0.5,0.5,0.5,0.6),order='xyz'):
        '''
        创建一个Box
        :param xSize:
        :param ySize:
        :param zSize:
        :param x:
        :param y:
        :param z:
        :param roll:
        :param pitch:
        :param yaw:
        :param color:
        :return:
        '''
        vertices = np.array([
            [1, 1, 1],
            [-1, 1, 1],
            [-1, -1, 1],
            [1, -1, 1],
            [1, 1, -1],
            [-1, 1, -1],
            [-1, -1, -1],
            [1, -1, -1]
        ],float)
        vertices *= np.array(size) / 2
        vertices = vertices.transpose()

        vertices=basic_math.geolib.EulerMatrix(roll,pitch,yaw,order)@vertices

        vertices=vertices.transpose()
        faces = np.array([
            [0, 1, 2],
            [2, 3, 0],
            [4, 5, 6],
            [6, 7, 4],
            [0, 3, 7],
            [ 7, 4, 0],
            [1, 2, 6],
            [6, 5, 1],
            [0, 1, 5],
            [5, 4, 0],
            [2, 3, 7],
            [7, 6, 2]
        ], dtype=np.uint)
        box = gl.GLMeshItem(vertexes=vertices, faces=faces, drawEdges=True, smooth=False)
        box.translate(pos[0],pos[1],pos[2])
        box.setColor(color)
        return box